//aeroQuad serial command
#include <math.h>
#define VERSION 2.4
  #define SERIAL_READ       SerialUSB.read
  #define SERIAL_PRINT      SerialUSB.print
   #define SERIAL_PRINTLN      SerialUSB.println
  #define SERIAL_AVAILABLE  SerialUSB.available
  HardwareSerial *binaryPort;
  char queryType = 'X';
  byte update = 0;
  void comma() {
  SERIAL_PRINT(',');
}
  void PrintValueComma(float val) {
  SERIAL_PRINT(val);
  comma();
}

void PrintValueComma(double val) {
  SERIAL_PRINT(val);
  comma();
}

void PrintValueComma(char val) {
  SERIAL_PRINT(val);
  comma();
}

void PrintValueComma(int val) {
  SERIAL_PRINT(val);
  comma();
}

void PrintValueComma(unsigned long val)
{
  SERIAL_PRINT(val);
  comma();
}
void PrintPID(unsigned char IDPid)
{
  PrintValueComma(IDPid);
  PrintValueComma(IDPid);
  PrintValueComma(IDPid);
}
void ReadSerialCommand()
{
  if(SerialUSB.available())
  {
    queryType = SERIAL_READ();
     switch (queryType) {
     case 'A':
       break;
      case 'C':
       break;
      case 'E':
       break;
     case 'G':
       break;
     case 'I':
       break;  
      case 'K':
       break;
      case 'M':
       break;
      
     case 'O':
       break;
      
      case 'W':
       break; 
       case 'Y':
        break;
       case '1':
       break;
       
       case '2':
       break;
       case '3':
       break; 
        case '4':
       break;
        case '5':
       break;
       case 'a':
       break;
        case 'b':
       break;
        case 'c':
       break;
        case 'd':
       break;
        case 'f':
       break;
        case '~':
       break;
       
     }
     
  }
}
void sendSerialTelemetry() {
  update = 0;
  switch (queryType) {
    case '='://// Reserved debug command to view any variable from Serial Monitor
    break;
    case 'B'://发送 roll pich 陀螺PID值
    PrintPID(100);
    PrintPID(100);
    SERIAL_PRINTLN(1);
    queryType = 'X';
    break;
    case 'D'://发送yaw PID值
    PrintPID(100);
    PrintPID(100);
    SERIAL_PRINTLN(8, BIN);
    queryType = 'X';
    break;
    case 'F':// Send roll and pitch auto level PID values
     PrintPID(3);
    PrintPID(4);
    PrintPID(6);
    PrintPID(7);
    SERIAL_PRINTLN(0.375);
    queryType = 'X';
    break;
    case 'H':// Send auto level configuration values
    PrintValueComma(1000);
    SERIAL_PRINTLN(100);
    queryType = 'X';
    break;
    case 'J':// Altitude Hold
    for(byte i=0; i<9; i++) {
     PrintValueComma(0);
    }
    SERIAL_PRINTLN('0');
    queryType = 'X';
    break;
    case 'L':// Send data filtering values
    PrintValueComma(0);
    PrintValueComma(0);
    SERIAL_PRINTLN(5);
    queryType = 'X';
    break;
    case 'N': // Send transmitter smoothing values
     PrintValueComma(100);
    for (byte axis = 0; axis < 5; axis++) {
      PrintValueComma(200);
    }
    SERIAL_PRINTLN(200);
    queryType = 'X';
    break;
    case 'P':// Send transmitter calibration data
     for (byte axis = 0; axis < 5; axis++) {
      PrintValueComma(100);
      PrintValueComma(10);
    }
    PrintValueComma(100);
    SERIAL_PRINTLN(12);
    queryType = 'X';
    break;
    case 'Q':// Send sensor data
       #if 1
      PrintValueComma(RollGyroValue);
       PrintValueComma(PitchGyroValue);
        PrintValueComma(YawGyroValue);
         PrintValueComma(xValue);
         PrintValueComma(yValue);
         PrintValueComma(zValue);
        #else
     PrintValueComma(accVector[0]);
     PrintValueComma(accVector[1]);
     PrintValueComma(accVector[2]);
      PrintValueComma(accXZAngle);
     PrintValueComma(accYZAngle);
     PrintValueComma(zValue);
     #endif
     
     
    for (byte axis = 0; axis < 2; axis++) {
//      PrintValueComma(levelAdjust[axis]);
      PrintValueComma(Awz[axis]);
    }
  
    #ifdef   FlightAngle_ARG_MODE
    PrintValueComma(degrees(flightAngle->getData(ROLL)));
    PrintValueComma(degrees(flightAngle->getData(PITCH)));
    #else
     PrintValueComma(degrees(angle[ROLL]) );
     PrintValueComma(degrees(angle[PITCH]) );
     //PrintValueComma(degrees(atan2(RwEst[0],RwEst[2])));
     //PrintValueComma(degrees(atan2(RwEst[1],RwEst[2])));
    #endif
    
      PrintValueComma(0);
     
    
      PrintValueComma(0);
    
      SERIAL_PRINT(0);
   
    SERIAL_PRINTLN();
     
    break;
    case 'r'://复位数据
    initRam();
    queryType = 'X';
    break;
    case 'R':// 原始测量数据
    PrintValueComma(xValue);
    PrintValueComma(yValue);
    PrintValueComma(zValue);
    PrintValueComma(0);
    PrintValueComma(0);    
    SERIAL_PRINTLN(0);
    queryType = 'X';
    break;
    case 'S'://发送所有飞行数据
    PrintValueComma(2);
    PrintValueComma(-10);
    PrintValueComma(3);
    PrintValueComma(-2);
    PrintValueComma(1011);   
   PrintValueComma(1012);    
   PrintValueComma(1002);   
   PrintValueComma(1000);   
   PrintValueComma(1001);   
   PrintValueComma(1003);  
  PrintValueComma(1002);    
    SERIAL_PRINTLN(1004);
    queryType = 'X';
    break;
    case 'T':// Send processed transmitter values
     PrintValueComma(1520);  
  PrintValueComma(1490);    
    SERIAL_PRINTLN(1500);
    queryType = 'X';
    break;
    case 'U':// Send smoothed receiver with Transmitter Factor applied values
     PrintValueComma(1);    
   PrintValueComma(1002);   
   PrintValueComma(1000);   
   PrintValueComma(1001);   
   PrintValueComma(1003);  
  PrintValueComma(1002);    
    SERIAL_PRINTLN(1004);
    queryType = 'X';
    break;
    case 'V':// Send receiver status
    for (byte channel = 0; channel < 5; channel++) {
      PrintValueComma(0);
    }
    SERIAL_PRINTLN(0);
    
    break;
    case 'X':// Stop sending messages
    queryType = 'X';
    break;
    case 'Z':// Send heading
    queryType = 'X';
    break;
    case '6':// Report remote commands
    queryType = 'X';
    break;
    case '!'://Send flight software version
     SERIAL_PRINTLN(VERSION);
    queryType = 'X';
    break;
    case '#':// Send software configuration
     PrintValueComma(4);
     SERIAL_PRINT('0');
     SERIAL_PRINTLN();
    queryType = 'X';
    break;
    case 'e':// Send AREF value
    queryType = 'X';
    break;
    case 'g': // Send magnetometer cal values
    queryType = 'X';
    break;
    case '`':// Send Camera values 
     
    break;
  }
 }
  
  // Used to read floating point values from the serial port
float readFloatSerial() {
  #define SERIALFLOATSIZE 10
  byte index = 0;
  byte timeout = 0;
  char data[SERIALFLOATSIZE] = "";

  do {
    if (SERIAL_AVAILABLE() == 0) {
      delay(10);
      timeout++;
    }
    else {
      data[index] = SERIAL_READ();
      timeout = 0;
      index++;
    }
  }  
  while ((index == 0 || data[index-1] != ';') && (timeout < 5) && (index < sizeof(data)-1));
  data[index] = '\0';
  return atof(data);
}



void printInt(int data) {
  byte msb, lsb;

  msb = data >> 8;
  lsb = data & 0xff;

  binaryPort->print(msb, BYTE);
  binaryPort->print(lsb, BYTE);
}

void sendBinaryFloat(float data) {
  union binaryFloatType {
    byte floatByte[4];
    float floatVal;
  } binaryFloat;
  
  binaryFloat.floatVal = data;
  binaryPort->print(binaryFloat.floatByte[3], BYTE);
  binaryPort->print(binaryFloat.floatByte[2], BYTE);
  binaryPort->print(binaryFloat.floatByte[1], BYTE);
  binaryPort->print(binaryFloat.floatByte[0], BYTE);
}

void sendBinaryuslong(unsigned long data) {
  union binaryuslongType {
    byte uslongByte[4];
    unsigned long uslongVal;
  } binaryuslong;
  
  binaryuslong.uslongVal = data;
  binaryPort->print(binaryuslong.uslongByte[3], BYTE);
  binaryPort->print(binaryuslong.uslongByte[2], BYTE);
  binaryPort->print(binaryuslong.uslongByte[1], BYTE);
  binaryPort->print(binaryuslong.uslongByte[0], BYTE);
}
void serialize16(int16 a) 
{
  SerialUSB.write(a);
  SerialUSB.write( a>>8&0xff);
 
}
void serialize8(uint8 a) 
{
  SerialUSB.write(a);
}
void multiWiiserialCom() {
  int16 a;
  uint8 i;

  uint16 intPowerMeterSum, intPowerTrigger1;   

  if ( SERIAL_AVAILABLE() == 0) {
    switch (SERIAL_READ()) {
   
    case 'M': // Multiwii @ arduino to GUI all data
     
      serialize8('M');
      serialize8(18);  // MultiWii Firmware version
     // serialize16(RxCh2); 
      //serialize16(RxCh3); 
      //serialize16(RxCh4); 
     
     for(i=0;i<3;i++)serialize16(accADC[i]);
      for(i=0;i<3;i++) serialize16(gyroADC[i]/8);
      for(i=0;i<3;i++) serialize16(0);
      serialize16(EstAlt*100.0f);
      serialize16(0); // compass
      for(i=0;i<4;i++) serialize16(0);
      for(i=0;i<6;i++) serialize16(0);
      for(i=0;i<8;i++) serialize16(rcValue[i]);
      serialize8(1<<1);
      serialize8(0);
      serialize16(deltaTime);
      for(i=0;i<2;i++) serialize16(degrees(flightAngle->getData(i)));
      serialize8(3);
      for(i=0;i<5;i++) {serialize8(P8[i]);serialize8(I8[i]);serialize8(0);}
      serialize8(P8[PIDLEVEL]);serialize8(I8[PIDLEVEL]);
      serialize8(P8[PIDMAG]);
      serialize8(rcRate8); serialize8(rcExpo8);
      serialize8(rollPitchRate); serialize8(yawRate);
      serialize8(dynThrPID);
      for(i=0;i<6;i++) serialize8(0);
      
      serialize16(33);
      serialize16(RxCh2);
      serialize8(99);
      serialize16(1*100.0f); // 4 variables are here for general monitoring purpose
      
      serialize8('M');
      //UartSendData(); // Serial.write(s,point);
      break;
   
   
  
    }
  }
}

